package FRC2856.Robot;

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/




//import com.sun.squawk.microedition.io.SocketConnection;
import edu.wpi.first.wpilibj.*;
import java.io.*;
import java.util.*;
import javax.microedition.io.Connector;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotCoding {
    private Hashtable vars;
    public Hashtable modes = new Hashtable();
    private Vector boolStack = new Vector();
    private Vector boolStackFuncs = new Vector();
    private Vector loopFuncs = new Vector();
    private Vector loopTruths= new Vector();

    public RobotCoding()
    {
        vars = new Hashtable();        
    }

    public void parse(String in)
    {
        parse(in, false);
    }
    public void parse(String input, boolean looping)
    {
        if(!input.trim().equals(""))
        {
            Vector code = RobotCoding.split(input, ";");

            for(int myIterativeVariable = 0; myIterativeVariable < code.size(); myIterativeVariable ++)
            {
                String in = (String) code.elementAt(myIterativeVariable);
                if(in.trim().equals(""))
                    continue;
//                System.out.println(in);

                Vector[] params = parameterize(in);
                Vector cmds = params[0];
                Vector cmdParams = params[1];

                if(!looping)
                    for(int i = 0; i < loopFuncs.size(); i ++)
                    {
                        loopFuncs.setElementAt(loopFuncs.elementAt(i) + in + ";", i);
                    }
        //        System.out.println("("+boolStackFuncs+")");

        //        System.out.println(">" + in + ": " + evaluateBooleanStack());

                if(cmds.elementAt(0).equals("end"))
                {
                    String func = (String) boolStackFuncs.elementAt(boolStackFuncs.size()-1);
        //            boolean truthiness = ((Boolean) boolStack.elementAt(boolStack.size()-1)).booleanValue();
                    if(func.indexOf("while") != -1)
                    {
                        int i = Integer.parseInt((String) func.substring(func.indexOf("-")+1));
        //                System.out.println("testing: " + (String) loopTruths.elementAt(i));

                        Vector[] newParams = parameterize((String) loopTruths.elementAt(i));
                        Vector newCmdParams = newParams[1];
                        Vector v = split((String) newCmdParams.elementAt(0), " ");
                        boolean newTruth = evaluateBooleanExpression(v);
        //                System.out.println("testing / " + newTruth);

                        if(newTruth)
                        {
        //                    System.out.println("<--" + (String) loopFuncs.elementAt(i) + "-->");
                            Vector lines = split((String) loopFuncs.elementAt(i), ";");

        //                    System.out.println("START LOOP : " + lines);
                            for(int p = 0; p < lines.size(); p ++)
                            {
                                String line = (String) lines.elementAt(p);
        //                        System.out.println("-->" + line);
                                if(!line.trim().equals(""))
                                    parse(line, true);
                            }
        //                    System.out.println("END LOOP");
                        }
                        else
                        {
                            loopTruths.removeElementAt(i);
                            loopFuncs.removeElementAt(i);
                            boolStack.removeElementAt(boolStack.size()-1);
                            boolStackFuncs.removeElementAt(boolStackFuncs.size()-1);
                        }
                    }
                    else
                    {
                        boolStack.removeElementAt(boolStack.size()-1);
                        boolStackFuncs.removeElementAt(boolStackFuncs.size()-1);
                    }
                }
                else if(!evaluateBooleanStack())
                    continue;
                else if(cmds.elementAt(0).equals("declare"))
                {
                    String varDef = (String) cmds.elementAt(1);
                    String varName= (String) cmds.elementAt(2);
                    Object obj = null;

                    if(varDef.equals("solenoid"))
                    {
                        if(cmdParams.size() == 1)
                            obj = new Solenoid( parseInt(cmdParams.elementAt(0)) );
                        else if(cmdParams.size() == 2)
                            obj = new Solenoid( parseInt(cmdParams.elementAt(0)), parseInt(cmdParams.elementAt(1)) );
                    }
                    else if(varDef.equals("compressor"))
                    {
                        if(cmdParams.size() == 2)
                            obj = new Compressor( parseInt(cmdParams.elementAt(0)), parseInt(cmdParams.elementAt(1)) );
                        else if(cmdParams.size() == 4)
                            obj = new Compressor( parseInt(cmdParams.elementAt(0)), parseInt(cmdParams.elementAt(1)), parseInt(cmdParams.elementAt(2)), parseInt(cmdParams.elementAt(3)) );
                    }
                    else if(varDef.equals("jaguar"))
                    {
                        if(cmdParams.size() == 1)
                            obj = new Jaguar( parseInt(cmdParams.elementAt(0)) );
                        else if(cmdParams.size() == 2)
                            obj = new Jaguar( parseInt(cmdParams.elementAt(0)), parseInt(cmdParams.elementAt(1)) );
                    }
                    else if(varDef.equals("servo"))
                    {
                        if(cmdParams.size() == 1)
                            obj = new Servo( parseInt(cmdParams.elementAt(0)) );
                        else if(cmdParams.size() == 2)
                            obj = new Servo( parseInt(cmdParams.elementAt(0)), parseInt(cmdParams.elementAt(1)) );
                    }
                    else if(varDef.equals("joystick"))
                    {
                        if(cmdParams.size() == 1)
                            obj = new Joystick( parseInt(cmdParams.elementAt(0)) );
                    }
                    else if(varDef.equals("robotdrive"))
                    {
                        if(cmdParams.size() == 2)
                            obj = new RobotDrive( parseInt(cmdParams.elementAt(0)), parseInt(cmdParams.elementAt(1)) );
                        else if(cmdParams.size() == 4)
                            obj = new RobotDrive( parseInt(cmdParams.elementAt(0)), parseInt(cmdParams.elementAt(1)), parseInt(cmdParams.elementAt(2)), parseInt(cmdParams.elementAt(3)) );
                    }
                    else if(varDef.equals("string"))
                    {
                        if(cmdParams.size() == 0)
                            obj = new String();
                        else if(cmdParams.size() == 1)
                            obj = new String((String) cmdParams.elementAt(0));
                    }
                    else if(varDef.equals("int"))
                    {
                        if(cmdParams.size() == 0)
                            obj = new Integer(0);
                        else if(cmdParams.size() == 1)
                            obj = new Integer(parseInt(cmdParams.elementAt(0)));
                    }
                    else if(varDef.equals("boolean"))
                    {
                        if(cmdParams.size() == 0)
                            obj = new Boolean(false);
                        else if(cmdParams.size() == 1)
                            obj = new Boolean(parseBoolean(cmdParams.elementAt(0)));
                    }
                    else if(varDef.equals("double"))
                    {
                        if(cmdParams.size() == 0)
                            obj = new Double(0);
                        else if(cmdParams.size() == 1)
                            obj = new Double(parseDouble(cmdParams.elementAt(0)));
                    }

                    vars.put(varName, obj);
                }
                else if(cmds.elementAt(0).equals("if"))
                {
                    Vector v = split((String) cmdParams.elementAt(0), " ");
                    boolean truthiness = evaluateBooleanExpression(v);
                    boolStack.addElement(truthiness?Boolean.TRUE:Boolean.FALSE);
                    boolStackFuncs.addElement("if");
                }
                else if(cmds.elementAt(0).equals("while"))
                {
                    Vector v = split((String) cmdParams.elementAt(0), " ");
                    boolean truthiness = evaluateBooleanExpression(v);
                    boolStack.addElement(truthiness?Boolean.TRUE:Boolean.FALSE);
                    boolStackFuncs.addElement("while-" + loopFuncs.size());
                    loopFuncs.addElement("");
                    loopTruths.addElement(in);
                }
                else if(cmds.elementAt(0).equals("print"))
                {
                    System.out.println((String) cmdParams.elementAt(0));
                }
                else if(cmds.elementAt(0).equals("add"))
                {
                    String varName = (String) cmdParams.elementAt(0);
                    int incBy = parseInt(cmdParams.elementAt(1));
                    Object obj = vars.get(varName);

                    if(obj instanceof Integer)
                        vars.put(varName, new Integer(((Integer)obj).intValue() + incBy));
                    else if(obj instanceof Double)
                        vars.put(varName, new Double(((Double)obj).doubleValue() + incBy));
                }
                else
                {
                    String varName = (String) cmds.elementAt(0);
                    String method  = (String) cmds.elementAt(1);
                    Object obj = vars.get(varName);

                    if(obj instanceof Solenoid)
                    {
                        Solenoid realObj = (Solenoid) obj;
                        if(method.equals("set"))
                            realObj.set(parseBoolean(cmdParams.elementAt(0)));
                    }
                    else if(obj instanceof Compressor)
                    {
                        Compressor realObj = (Compressor) obj;
                        if(method.equals("start"))
                            realObj.start();
                        else if(method.equals("stop"))
                            realObj.stop();
                    }
                    else if(obj instanceof Jaguar)
                    {
                        Jaguar realObj = (Jaguar) obj;
                        if(method.equals("set"))
                            realObj.set(parseDouble(cmdParams.elementAt(0)));
                    }
                    else if(obj instanceof Servo)
                    {
                        Servo realObj = (Servo) obj;
                        if(method.equals("set"))
                            realObj.set(parseDouble(cmdParams.elementAt(0)));
                        else if(method.equals("setAngle"))
                            realObj.setAngle(parseDouble(cmdParams.elementAt(0)));
                    }
                    else if(obj instanceof RobotDrive)
                    {
                        RobotDrive realObj = (RobotDrive) obj;
                        if(method.equals("arcadeDrive") && cmdParams.size() == 2)
                            realObj.arcadeDrive(parseDouble(cmdParams.elementAt(0)), parseDouble(cmdParams.elementAt(1)));
                        else if(method.equals("arcadeDrive") && cmdParams.size() == 1)
                            realObj.arcadeDrive( (Joystick) vars.get((String)cmdParams.elementAt(0)));
                        else if(method.equals("drive"))
                            realObj.drive(parseDouble(cmdParams.elementAt(0)), parseDouble(cmdParams.elementAt(1)));
                    }
                }
            }
        }
        
        
    }

    /** Evaluates our current boolean stack to see if we're TRUE */
    public boolean evaluateBooleanStack()
    {
        for(int i = 0; i < boolStack.size(); i ++)
            if(! ((Boolean)boolStack.elementAt(i)).booleanValue())
                return false;

        return true;
    }

    /** Evaluates a boolean expression as found in an if() or while() statement. */
    public boolean evaluateBooleanExpression(Vector v)
    {
        int truthiness = 1;
        int op = 0; // 0=AND, 1=OR
        int index;
        for(int i = 0; i < v.size(); i ++)
            {
                String s = ((String) v.elementAt(i)).trim();
                String p1, p2;

                if(i % 2 == 0)
                {
                    if( (index=s.indexOf("==")) != -1)
                    {
                        p1 = s.substring(0, index);
                        p2 = s.substring(index+2);

                        if(op==0)
                            truthiness &= p1.equals(p2) ? 1 : 0;
                        else if(op==1)
                            truthiness |= p1.equals(p2) ? 1 : 0;
                    }
                    else if( (index=s.indexOf("!=")) != -1)
                    {
                        p1 = s.substring(0, index);
                        p2 = s.substring(index+2);

                        if(op==0)
                            truthiness &= !p1.equals(p2) ? 1 : 0;
                        else if(op==1)
                            truthiness |= !p1.equals(p2) ? 1 : 0;
//                        System.out.println("truth: " + truthiness);
                    }
                    else if((index=s.indexOf(">=")) != -1)
                    {
                        p1 = s.substring(0, index);
                        p2 = s.substring(index+2);

                        double d1 = Double.parseDouble(p1);
                        double d2 = Double.parseDouble(p2);
                        if(op==0)
                            truthiness &= d1>=d2 ? 1 : 0;
                        else if(op==1)
                            truthiness |= d1>=d2 ? 1 : 0;
                    }
                    else if((index=s.indexOf("<=")) != -1)
                    {
                        p1 = s.substring(0, index);
                        p2 = s.substring(index+2);

                        double d1 = Double.parseDouble(p1);
                        double d2 = Double.parseDouble(p2);
                        if(op==0)
                            truthiness &= d1<=d2 ? 1 : 0;
                        else if(op==1)
                            truthiness |= d1<=d2 ? 1 : 0;
                    }
                    else if((index=s.indexOf(">")) != -1)
                    {
                        p1 = s.substring(0, index);
                        p2 = s.substring(index+1);

                        double d1 = Double.parseDouble(p1);
                        double d2 = Double.parseDouble(p2);
                        if(op==0)
                            truthiness &= d1>d2 ? 1 : 0;
                        else if(op==1)
                            truthiness |= d1>d2 ? 1 : 0;
                    }
                    else if((index=s.indexOf("<")) != -1)
                    {
                        p1 = s.substring(0, index);
                        p2 = s.substring(index+1);

                        double d1 = Double.parseDouble(p1);
                        double d2 = Double.parseDouble(p2);
                        if(op==0)
                            truthiness &= d1<d2 ? 1 : 0;
                        else if(op==1)
                            truthiness |= d1<d2 ? 1 : 0;
                    }

                }
                else
                {
                    if(s.equals("&&"))
                        op=0;
                    else if(s.equals("||"))
                        op=1;
                }
            }
        return truthiness == 1;
    }

    public Vector[] parameterize(String in)
    {
        Vector commandVector = new Vector();
        Vector paramVector = new Vector();
        int index = 0;

        while( (index=in.indexOf(" ")) != -1 && (in.indexOf("(") == -1 || index < in.indexOf("(")))
        {
            commandVector.addElement(in.substring(0, index).trim());
            in = in.substring(index+1);
        }

        if( (index=in.indexOf("(")) != -1)
        {
            paramVector = split(in.substring(index+1, in.indexOf(")")), ",");

            for(int p = 0; p < paramVector.size(); p ++)
            {
                int indx1,indx2;
                String s = (String) paramVector.elementAt(p);
                while( (indx1=s.indexOf("{")) != -1 && (indx2=s.indexOf("}")) != -1 && indx2 > indx1)
                {
                    s = s.substring(0, indx1) + vars.get(s.substring(indx1+1, indx2)) + s.substring(indx2+1);
                }
                paramVector.setElementAt(s, p);
            }
            in = in.substring(0, index);
        }

        if(!in.trim().equals(""))
        {
            commandVector.addElement(in);
        }

        return new Vector[]{ commandVector, paramVector };
    }

    public static Vector split(String s, String delim)
    {
        int index = 0;
        Vector ret = new Vector();

        while( (index = s.indexOf(delim)) != -1 )
        {
            ret.addElement(s.substring(0, index).trim());
            s = s.substring(index+1);
        }

        if(!s.trim().equals(""))
        {
            ret.addElement(s);
        }

        return ret;
    }

    public static int parseInt(Object s)
    {
        return Integer.parseInt((String) s);
    }
    public static boolean parseBoolean(Object s)
    {
        String str = (String) s;
        return str.equals("true");
    }
    public static double parseDouble(Object s)
    {
        return Double.parseDouble((String) s);
    }

    public static void main(String [] args) throws IOException
    {
        RobotCoding rc = new RobotCoding();
        rc.parse("declare int test(10);while({test}!=1); if({test}==2 || {test}==4 || {test}==6 || {test}==8);print({test});end;add(test,-1);end;print(newai)");
//        rc.parse("declare int test(10)");
//        rc.parse("while({test}!=1)");
//        rc.parse("if({test}==2 || {test}==4 || {test}==6 || {test}==8)");
//        rc.parse("print({test})");
//        rc.parse("end");
//        rc.parse("add(test,-1)");
//        rc.parse("end");
//        rc.parse("print(anyway)");
    }
}
